//
// Created by lqk on 2022/1/30.
//

#include "QuadrupedKinematics.h"

Eigen::Matrix3f LegKinematics::CalculateJacobian(const Eigen::Vector3f & q,int sideSign) {
    float l1 = m_leg.m_abadLinkLength;
    float l2 = m_leg.m_hipLinkLength;
    float l3 = m_leg.m_kneeLinkLength;

    float s1 = std::sin(q(0));
    float s2 = std::sin(q(1));
    float s3 = std::sin(q(2));

    float c1 = std::cos(q(0));
    float c2 = std::cos(q(1));
    float c3 = std::cos(q(2));

    float c23 = c2 * c3 - s2 * s3;
    float s23 = s2 * c3 + c2 * s3;

    Eigen::Matrix3f jac;
    jac.operator()(0,0)=0;
    jac.operator()(0, 1) = l3 * c23 + l2 * c2;
    jac.operator()(0, 2) = l3 * c23;
    jac.operator()(1, 0) =
            l3 * c1 * c23 + l2 * c1 * c2 - (l1) * sideSign * s1;
    jac.operator()(1, 1) = -l3 * s1 * s23 - l2 * s1 * s2;
    jac.operator()(1, 2) = -l3 * s1 * s23;
    jac.operator()(2, 0) =
            l3 * s1 * c23 + l2 * c2 * s1 + (l1) * sideSign * c1;
    jac.operator()(2, 1) = l3 * c1 * s23 + l2 * c1 * s2;
    jac.operator()(2, 2) = l3 * c1 * s23;

    return jac;
}

Eigen::Vector3f LegKinematics::CalculateForwardKinematics(const Eigen::Vector3f &q, int sideSign) {
    float l1 = m_leg.m_abadLinkLength;
    float l2 = m_leg.m_hipLinkLength;
    float l3 = m_leg.m_kneeLinkLength;

    float s1 = std::sin(q(0));
    float s2 = std::sin(q(1));
    float s3 = std::sin(q(2));

    float c1 = std::cos(q(0));
    float c2 = std::cos(q(1));
    float c3 = std::cos(q(2));

    float c23 = c2 * c3 - s2 * s3;
    float s23 = s2 * c3 + c2 * s3;

    Eigen::Vector3f pos;
    pos.operator()(0) = l3 * s23 + l2 * s2;
    pos.operator()(1) = (l1) * sideSign * c1 + l3 * (s1 * c23) + l2 * c2 * s1;
    pos.operator()(2) = (l1) * sideSign * s1 - l3 * (c1 * c23) - l2 * c1 * c2;

    return pos;
}

void QuadrupedKinematics::CalculateJacobians(LegState* const legsState) {
    legsState[0].m_jacobian=m_legFRKinematics.CalculateJacobian(legsState[0].m_q,-1);
    legsState[1].m_jacobian=m_legFLKinematics.CalculateJacobian(legsState[1].m_q,1);
    legsState[2].m_jacobian=m_legBRKinematics.CalculateJacobian(legsState[2].m_q,-1);
    legsState[3].m_jacobian=m_legBLKinematics.CalculateJacobian(legsState[3].m_q,1);
}

void QuadrupedKinematics::CalculateFootsPosition(LegState* const legsState) {
    legsState[0].m_footPos=m_legFRKinematics.CalculateForwardKinematics(legsState[0].m_q,-1);
    legsState[1].m_footPos=m_legFLKinematics.CalculateForwardKinematics(legsState[1].m_q,1);
    legsState[2].m_footPos=m_legBRKinematics.CalculateForwardKinematics(legsState[2].m_q,-1);
    legsState[3].m_footPos=m_legBLKinematics.CalculateForwardKinematics(legsState[3].m_q,1);
}

Eigen::Vector3f QuadrupedKinematics::GetHipLocation(int leg) {
    assert(leg >= 0 && leg < 4);
    Eigen::Vector3f pHip((leg==0||leg==1) ? m_quadSpec.m_bodyLength/2. : -m_quadSpec.m_bodyLength/2.,
                         (leg==1||leg==3) ? m_quadSpec.m_bodyWidth/2.: -m_quadSpec.m_bodyWidth/2.,
                         0);
    return pHip;
}